/****************************************************************************
 *  Copyright (C) 2018 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/
/** @file modeswitch_task.c
 *  @version 1.0
 *  @date Oct 2017
 *
 *  @brief infantry control mode switch
 *
 *  @copyright 2017 DJI RoboMaster. All rights reserved.
 *
 */
 
#include "modeswitch_task.h"
#include "info_get_task.h"
#include "remote_ctrl.h"
#include "gimbal_task.h"
#include "chassis_task.h"
#include "comm_task.h"
#include "shoot_task.h"
#include "detect_task.h"
#include "infantry_info.h"
#include "bsp_uart.h"
#include "keyboard.h"
#include "sys_config.h"
#include "math.h"
#include "stdlib.h"
#include "cmsis_os.h"
#include "function_mode_task.h"
#include "bsp_can.h"

/* stack usage monitor */
UBaseType_t mode_stack_surplus;

/* mode switch task static parameter */
infantry_mode_e last_glb_ctrl_mode;
infantry_mode_e glb_ctrl_mode;

extern TaskHandle_t info_get_task_t;
extern osTimerId chassis_timer_id;
extern osTimerId gimbal_timer_id;
extern osTimerId funct_timer_id;

extern float updown_ref_angle,rotate_ref_angle,hug_ref_angle;
extern float updown_ref_angle1,updown_ref_angle2,updown_ref_spd1,updown_ref_spd2;
void mode_switch_task(void const *argu)
{
//  gimbal_self_check();
  
	//开启两个实时任务
//  osTimerStart(gimbal_timer_id, GIMBAL_PERIOD);				//period = 1ms
	osTimerStart(funct_timer_id, FUNCTION_MODE_PERIOD);	
  osTimerStart(chassis_timer_id, CHASSIS_PERIOD);			//period = 10ms
	
  uint32_t mode_wake_time = osKernelSysTick();
  while (1)
  {
    taskENTER_CRITICAL();
    
		get_global_last_mode();
    get_main_ctrl_mode();
    
    get_chassis_mode();
//    get_gimbal_mode();
//    get_shoot_mode();
    get_function_mode();
    
    taskEXIT_CRITICAL();
    
    osSignalSet(info_get_task_t, INFO_GET_EXE_SIGNAL);
    
    mode_stack_surplus = uxTaskGetStackHighWaterMark(NULL);
    osDelayUntil(&mode_wake_time, INFO_GET_PERIOD);		//period = 5ms
  }

}

//static void kb_enable_hook(void)
//{
//  if (rc.sw1 == RC_MI && rc.sw2 == RC_UP)
//    km.kb_enable = 1;
//  else
//    km.kb_enable = 0;

//}

void get_main_ctrl_mode(void)
{
  //host PC has been connected
  if (!g_err.list[PC_SYS_OFFLINE].err_exist) //(!g_err.list[PC_SYS_OFFLINE].err_exist)
  {
    switch (rc.sw1)
    {
      case RC_UP:
      {
        glb_ctrl_mode = LAND_ISLAND_MODE;
      }break;
      
#ifdef AUTO_NAVIGATION
      case RC_MI:
      {
        glb_ctrl_mode = NORMAL_MOVE_MODE;
      }break;
      
      case RC_DN:
      {
        glb_ctrl_mode = PICK_BOX_MODE;
      }break;
#endif
      
      default:
      {
        glb_ctrl_mode = SAFETY_MODE;
      }break;
    }
  }
  //host PC offline
	
  else
  {
    switch (rc.sw1)
    {
      case RC_UP:
      {
        glb_ctrl_mode = LAND_ISLAND_MODE;
			}break;
			case RC_MI:
			{
				glb_ctrl_mode = NORMAL_MOVE_MODE;
			}break;
			case RC_DN:
      {
        glb_ctrl_mode = PICK_BOX_MODE;
      }break;
      default:
      {
        glb_ctrl_mode = SAFETY_MODE;
      }break;  
					
		}
  }
  
  if ((rc.sw1 == RC_DN) && (rc.sw2 == RC_DN))
    glb_ctrl_mode = SAFETY_MODE;
  
  //kb_enable_hook();
  
}


static action_mode_e remote_is_action(void)
{
  if ((abs(rc.ch1) >= 10)
   || (abs(rc.ch2) >= 10)
   || (abs(rc.ch3) >= 10)
   || (abs(rc.ch4) >= 10)
   || (abs(rc.mouse.x) >= 10)
   || (abs(rc.mouse.y) >= 10))
  {
    return IS_ACTION;
  }
  else
  {
    return NO_ACTION;
  }
}

//typedef enum
//{
//  GIMBAL_RELAX         = 0,
//  GIMBAL_INIT          = 1,
//  GIMBAL_NO_ARTI_INPUT = 2,
//  GIMBAL_FOLLOW_ZGYRO  = 3,
//  GIMBAL_TRACK_ARMOR   = 4,
//  GIMBAL_PATROL_MODE   = 5,
//  GIMBAL_SHOOT_BUFF    = 6,
//  GIMBAL_POSITION_MODE = 7,
//  GIMBAL_RELATIVE_MODE = 8,
//} gimbal_mode_e;
//static void gimbal_mode_handler(void)
//{
//  switch (glb_ctrl_mode)
//  {
//    case MANUAL_CTRL_MODE:
//    {
//      if (last_glb_ctrl_mode == SEMI_AUTO_MODE)
//        gim.ctrl_mode = GIMBAL_FOLLOW_ZGYRO;
//      
//			//遥控离线判断
//      /* no input control signal gimbal mode handle */
//      if (gim.input.ac_mode == NO_ACTION)
//      {
//        if (gim.ctrl_mode == GIMBAL_FOLLOW_ZGYRO)
//        {
//          //if (fabs(chassis.vw) <= gim.input.action_angle)
//          if (fabs(gim.sensor.yaw_relative_angle) <= gim.input.action_angle)
//          {
//            //begin no action handle
//            gim.ctrl_mode = GIMBAL_NO_ARTI_INPUT;
//            
//            gim.input.no_action_flag = 1;
//            gim.input.no_action_time = HAL_GetTick();
//          }
//        }
//      }
//      else  //IS_ACTION mode
//      {
//        chassis.follow_gimbal = 1;
//        if (gim.ctrl_mode == GIMBAL_NO_ARTI_INPUT)
//        {
//          gim.ctrl_mode = GIMBAL_FOLLOW_ZGYRO;
//          gim.input.no_action_flag = 0;
//          
//          gim.pid.yaw_angle_ref = 0;
//          gim.yaw_offset_angle = gim.sensor.yaw_gyro_angle;
//        }
//      
//				
//      /* manual trigger chassis twist */
//      if (km.twist_ctrl)
//        gim.ctrl_mode = GIMBAL_FOLLOW_ZGYRO;
//      
//#if 1
//      /* manual trigger track armor */		//自瞄
//      if (km.track_ctrl)
//        gim.ctrl_mode = GIMBAL_TRACK_ARMOR;
//      else
//			{
//				gim.ctrl_mode = GIMBAL_FOLLOW_ZGYRO;   
//			}	
//		}
//					
////      /* manual trigger big buff */	//大能量机关
////      if (km.buff_ctrl && km.kb_enable)
////      {
////        gim.ctrl_mode = GIMBAL_SHOOT_BUFF;
////        chassis.follow_gimbal = 0;
////        
////        if (gim.last_ctrl_mode != GIMBAL_SHOOT_BUFF)
////        {
////          gim.auto_ctrl_cmd = CMD_CALI_FIVE;
////        }
////      }
////      else
////        chassis.follow_gimbal = 1;
//#endif
//      
//      if (gim.last_ctrl_mode == GIMBAL_RELAX)
//        gim.ctrl_mode = GIMBAL_FOLLOW_ZGYRO;
//    }break;
//    
//    case SEMI_AUTO_MODE:
//    {
//      switch (rc.sw1)
//      {
//        case RC_UP:
//        {
//          gim.ctrl_mode = GIMBAL_TRACK_ARMOR;
//        }break;
//        
//        case RC_MI:
//        {
//          gim.ctrl_mode = GIMBAL_FOLLOW_ZGYRO;
//        }break;
//        
//        default:
//        {
//          gim.ctrl_mode = GIMBAL_RELATIVE_MODE;
//        }break;
//      }
//    }break;
//    
//    case AUTO_CTRL_MODE:
//    {
//      switch (rc.sw1)
//      {
//        case RC_UP:
//        case RC_MI:
//        {
//					gim.ctrl_mode = GIMBAL_RELAX;
////          //patrol and relative mode
////          gim.ctrl_mode = (gimbal_mode_e)pc_recv_mesg.gimbal_control_data.ctrl_mode;
////          
////          /* gimbal first enter patrol mode */
////          /* patrol valid only in gimbal auto mode */
////          if (gim.last_ctrl_mode != GIMBAL_PATROL_MODE && gim.ctrl_mode == GIMBAL_PATROL_MODE)
////          gim.pid.yaw_angle_ref = gim.sensor.yaw_relative_angle;
//        }break;
//        
//        default:
//        {
//          gim.ctrl_mode = GIMBAL_RELAX;
//        }break;
//      }
//    }break;
//    
//    default:
//    {
//      gim.ctrl_mode = GIMBAL_RELAX;
//    }break;
//  }
//}

//void get_gimbal_mode(void)
//{
//  gim.input.ac_mode = remote_is_action();
//  
//  if (gim.ctrl_mode != GIMBAL_INIT)
//  {
//    gimbal_mode_handler();
//  }
//  
//  /* gimbal back to center */
//  if ((gim.last_ctrl_mode == GIMBAL_RELAX) && (gim.ctrl_mode != GIMBAL_RELAX) && (glb_ctrl_mode != AUTO_CTRL_MODE))
//  {
//    /* set gimbal init mode */
//    gim.ctrl_mode = GIMBAL_INIT;
//    /* record yaw angle initial offset */
//    gim.ecd_offset_angle = gim.sensor.yaw_relative_angle;
//    /* set initial parameters */
//    gimbal_back_param();
//  }
//}

static void get_global_last_mode(void)
{
  last_glb_ctrl_mode = glb_ctrl_mode;
  gim.last_ctrl_mode = gim.ctrl_mode;
  chassis.last_ctrl_mode = chassis.ctrl_mode;
	function_mode.last_ctrl_mode = function_mode.ctrl_mode;
}

/*  CHASSIS_RELAX          = 0,
 *	CHASSIS_STOP           = 1,
 *	MANUAL_SEPARATE_GIMBAL = 2,
 *	MANUAL_FOLLOW_GIMBAL   = 3,
 *	DODGE_MODE             = 4,
 *	AUTO_SEPARATE_GIMBAL   = 5,
 *	AUTO_FOLLOW_GIMBAL     = 6,
 */
static void chassis_mode_handler(void)
{		
  switch (glb_ctrl_mode)
  {
    case LAND_ISLAND_MODE:
    {
			 switch (rc.sw2)
      {
        case RC_UP:
        {
          chassis.ctrl_mode = AUTO_UPDOWN_ISLAND_CHASSIS_ROTATE;
        }break;
        
        case RC_MI:
        {
          chassis.ctrl_mode = AUTO_UPDOWN_ISLAND_CHASSIS_ROTATE;
        }break;
        
        case RC_DN:
        {
          chassis.ctrl_mode = AUTO_UPDOWN_ISLAND_CHASSIS_ROTATE;
          //chassis.follow_gimbal = 1;
          //chassis.ctrl_mode = MANUAL_FOLLOW_GIMBAL;
        }break;
      }
//      
//      /* keyboard trigger chassis twist mode */
//      if (km.twist_ctrl||RC_DEFENSE)
//					chassis.ctrl_mode = DODGE_MODE;
    }break;
				
    case NORMAL_MOVE_MODE:
    {
      switch (rc.sw2)
      {
        case RC_UP:
        {
          chassis.ctrl_mode = SUPPLY_BALL_CHASSIS_MOVE;
        }break;
        
        case RC_MI:
        {
          chassis.ctrl_mode = MANUAL_SEPARATE_GIMBAL;
        }break;
        
        case RC_DN:
        {
          chassis.ctrl_mode = RESCUE_CHASSIS_MOVE;
          //chassis.follow_gimbal = 1;
          //chassis.ctrl_mode = MANUAL_FOLLOW_GIMBAL;
        }break;
        
      }
    }break;
    
    case PICK_BOX_MODE:
    {
      switch (rc.sw2)
      {
        case RC_UP:
        {
					chassis.ctrl_mode = PICK_BOX_CHASSIS_MOVE;
//        chassis.ctrl_mode = (chassis_mode_e)pc_recv_mesg.chassis_control_data.ctrl_mode;
        }break;
        case RC_MI:
        {
          chassis.ctrl_mode = PICK_BOX_CHASSIS_MOVE;
        }break;
        case RC_DN:
        {
          chassis.ctrl_mode = CHASSIS_STOP;
        }break;
        
      }
    }break;
    
    default:
    {
      chassis.ctrl_mode = CHASSIS_STOP;
    }break;
    
  }
  
}
//extern int8_t   twist_side;
//extern int8_t   twist_sign;
//extern uint32_t twist_count;

//extern int16_t twist_period;
//extern int16_t twist_angle;						//twist_angle = TWIST_ANGLE (40)
void get_chassis_mode(void)
{
//  if (gim.ctrl_mode == GIMBAL_INIT)
//  {
//    chassis.ctrl_mode = CHASSIS_STOP;
//  }
//  else
//  {
    chassis_mode_handler();
//  }
//  
//  /* chassis first enter dodge mode */
//  if (chassis.last_ctrl_mode != DODGE_MODE && chassis.ctrl_mode == DODGE_MODE)
//  {
//    if (gim.sensor.yaw_relative_angle > 0)
//      twist_side = 1;		//左边
//    else
//      twist_side = -1;	//右边
//    
//    if ((gim.sensor.yaw_relative_angle < twist_angle) && (gim.sensor.yaw_relative_angle > -twist_angle))
//      twist_count = 0;//acos((gim.sensor.yaw_relative_angle - twist_side*twist_angle)/(-twist_sign*40.0)) * twist_period / (2*PI);	// = 0
//      
//  }
  
}


/*  
ENGINEER_FUNCTION_RELAX = 0,
  ENGINEER_FUNCTION_STOP  = 1,
	AUTO_UP_ISLAND          = 2,
	MANUAL_UP_DOWN_ISLAND   = 3,
	AUTO_DOWN_ISLAND        = 4,
	SUPPLY_BALL             = 5,
	NORMAL_CHASSIS          = 6,
	RESCUE_MODE             = 7,
	AUTO_PICK_BOX           = 8,
	MANUAL_PICK_BOX         = 9,
*/
extern mcu_tx_data_t mcu_tx_data;
extern uint8_t rescue_camera_state;
uint8_t updown_reset_commit;
extern updown_island_mode_t updown_island_mode;

extern float debug_updown_island_angle1;
extern float debug_updown_island_angle2;
static void get_function_mode(void)
{
	switch (glb_ctrl_mode)
  {
    case LAND_ISLAND_MODE:
    {
			 switch (rc.sw2)
      {
        case RC_UP:
        {
          function_mode.ctrl_mode = AUTO_UP_ISLAND;
        }break;
        
        case RC_MI:
        {
          function_mode.ctrl_mode = MANUAL_UP_DOWN_ISLAND;
        }break;
        
        case RC_DN:
        {
          function_mode.ctrl_mode = AUTO_DOWN_ISLAND;
        }break;
      }
    }break;                                                       
				
    case NORMAL_MOVE_MODE:
    {
      switch (rc.sw2)
      {
        case RC_UP:
        {
          function_mode.ctrl_mode = SUPPLY_BALL;
        }break;
        
        case RC_MI:
        {
          function_mode.ctrl_mode = NORMAL_CHASSIS;
        }break;
        case RC_DN:
        {
          function_mode.ctrl_mode = RESCUE_MODE;
        }break;
        
      }
    }break;
    
    case PICK_BOX_MODE:
    {
      switch (rc.sw2)
      {
        case RC_UP:
        {
					function_mode.ctrl_mode = PICK_UP_BOX;
        }break;
        case RC_MI:
        {
          function_mode.ctrl_mode = PICK_DOWN_BOX;
        }break;
        case RC_DN:
        {
          function_mode.ctrl_mode = ENGINEER_FUNCTION_STOP;
        }break;
        
      }
    }break;
    
    default:
    {
      function_mode.ctrl_mode = ENGINEER_FUNCTION_STOP;
    }break;
    
  }

//初始化进入各个模式时电机位置
//	if ((function_mode.last_ctrl_mode != AUTO_UP_ISLAND) && (function_mode.ctrl_mode == AUTO_UP_ISLAND))
//	{
//		updown_ref_angle1 = 0;
//		updown_ref_angle2 = 0;
//	}
//	if ((function_mode.last_ctrl_mode != MANUAL_UP_DOWN_ISLAND) && (function_mode.ctrl_mode == MANUAL_UP_DOWN_ISLAND))
//	{
//		updown_ref_angle1 = 0;
//		updown_ref_angle2 = 0;
//	}
//	if ((function_mode.last_ctrl_mode != AUTO_DOWN_ISLAND) && (function_mode.ctrl_mode == AUTO_DOWN_ISLAND))
//	{
//		updown_ref_angle1 = 0;
//		updown_ref_angle2 = 0;
//	}
//	if ((function_mode.last_ctrl_mode != SUPPLY_BALL) && (function_mode.ctrl_mode == SUPPLY_BALL))
//	{
//		updown_ref_angle1 = 0;
//		updown_ref_angle2 = 0;
//	}
//	if ((function_mode.last_ctrl_mode != PICK_UP_BOX) && (function_mode.ctrl_mode == PICK_UP_BOX))
//	{
//		updown_ref_angle1 = 0;
//		updown_ref_angle2 = 0;
//	}
//	if ((function_mode.last_ctrl_mode != PICK_DOWN_BOX) && (function_mode.ctrl_mode == PICK_DOWN_BOX))
//	{
//		updown_ref_angle1 = 0;
//		updown_ref_angle2 = 0;
//	}
	
	if(function_mode.last_ctrl_mode != function_mode.ctrl_mode)
	{
		AIR_FAN_1 = 1000;
		AIR_FAN_2 = 1000;
		mcu_tx_data.updown_flag = 0;
		if(mcu_rx_data.pick_box_flag == 0)
		{
			switch(function_mode.ctrl_mode)
			{
				case AUTO_DOWN_ISLAND:
				{
					updown_ref_angle1 = UPDOWN_ISLAND_ANGLE2;
					updown_ref_angle2 = -UPDOWN_ISLAND_ANGLE2;
					updown_island_mode = Ready_to_down;
					updown_reset_commit = 0;
				}break;
				case MANUAL_UP_DOWN_ISLAND:
				{
					updown_ref_angle1 = 0;
					updown_ref_angle2 = 0;
					updown_reset_commit = 0;
				}break;
				case AUTO_UP_ISLAND:
				{
					updown_ref_angle1 = UPDOWN_ISLAND_ANGLE1;
					updown_ref_angle2 = -UPDOWN_ISLAND_ANGLE1;
					updown_island_mode = Ready_to_down;
					updown_reset_commit = 0;
				}break;
			}
		}
		else
		{
			updown_reset_commit = 1;
		}
	}
	
	if(updown_reset_commit == 1)
	{
		if(mcu_rx_data.pick_box_flag == 0)
		{
			switch(function_mode.ctrl_mode)
			{
				case AUTO_DOWN_ISLAND:
				{
					updown_ref_angle1 = UPDOWN_ISLAND_ANGLE2;
					updown_ref_angle2 = -UPDOWN_ISLAND_ANGLE2;
					updown_island_mode = Ready_to_down;
					updown_reset_commit = 0;
				}break;
				case MANUAL_UP_DOWN_ISLAND:
				{
					updown_ref_angle1 = 0;
					updown_ref_angle2 = 0;
					updown_reset_commit = 0;
				}break;
				case AUTO_UP_ISLAND:
				{
					updown_ref_angle1 = UPDOWN_ISLAND_ANGLE1;
					updown_ref_angle2 = -UPDOWN_ISLAND_ANGLE1;
					updown_island_mode = Ready_to_down;
					updown_reset_commit = 0;
				}break;
			}
		}
	}
	
	if(function_mode.ctrl_mode != RESCUE_MODE)
	{
		HAL_GPIO_WritePin(GPIOH, GPIO_PIN_2, GPIO_PIN_RESET);
		rescue_camera_state = 0;
	}
}	

uint8_t gimbal_is_controllable(void)
{
//  if (gim.ctrl_mode == GIMBAL_RELAX
//   || g_err.list[REMOTE_CTRL_OFFLINE].err_exist
//   || g_err.list[GIMBAL_YAW_OFFLINE].err_exist
//   || g_err.list[GIMBAL_PIT_OFFLINE].err_exist)
//    return 0;
//  else
//    return 1;
}

uint8_t chassis_is_controllable(void)
{
  if (chassis.ctrl_mode == CHASSIS_RELAX 
   || g_err.list[REMOTE_CTRL_OFFLINE].err_exist)
    return 0;
  else
    return 1;
}

//void get_shoot_mode(void)
//{
//  switch (glb_ctrl_mode)
//  {
//    case MANUAL_CTRL_MODE:
//    {
//      if (km.kb_enable)
//        shoot.ctrl_mode = KEYBOARD_CTRL_SHOT;
//      else
//        shoot.ctrl_mode = REMOTE_CTRL_SHOT;
//    }break;
//    
//    case SEMI_AUTO_MODE:
//    {
//      shoot.ctrl_mode = SEMIAUTO_CTRL_SHOT;
//      //shoot.ctrl_mode = SHOT_DISABLE;
//    }break;
//    
//    case AUTO_CTRL_MODE:
//    {
//      shoot.ctrl_mode = AUTO_CTRL_SHOT;			
//			if(RC_CTRL_BALL_STOR)
//				shoot.ball_storage_open = !shoot.ball_storage_open;
//    }break;
//    
//    default:
//    {
//      shoot.ctrl_mode = SHOT_DISABLE;
//    }break;
//    
//  }

//  if (gim.ctrl_mode == GIMBAL_RELAX)
//    shoot.ctrl_mode = SHOT_DISABLE;

//}
